#include "Filter_Algorithm.h"


// 基于二维伽马函数的光照不均匀图像自适应校正算法
// 基于HSV颜色空间的亮度提升算法
cv::Mat work_pro(const cv::Mat& src) {
    CV_Assert(!src.empty() && src.type() == CV_8UC3);
    cv::Mat hsv;
    cv::cvtColor(src, hsv, cv::COLOR_BGR2HSV);

    std::vector<cv::Mat> channels(3);
    cv::split(hsv, channels);

    cv::Mat V_float;
    channels[2].convertTo(V_float, CV_32F);

    // 计算高斯模糊
    const int kernel_size = std::min(src.rows, src.cols) | 1;
    constexpr float q = 1.4142135623730950488f;    // sqrt(2.0)
    constexpr float sigmas[] = {15.0f, 80.0f, 250.0f};

    cv::Mat F = cv::Mat::zeros(src.size(), CV_32FC1);
    for (const float sigma : sigmas) {
        cv::Mat temp;
        cv::GaussianBlur(V_float, temp, cv::Size(kernel_size, kernel_size), sigma / q);
        F += temp;
    }
    F /= 3.0f;

    // 计算平均亮度并调整V通道
    const float average = static_cast<float>(cv::mean(F)[0]);
    auto* pV = V_float.ptr<float>();
    const float* pF = F.ptr<float>();
    const int total = V_float.rows * V_float.cols;
    const float inv_average = 1.0f / average;

    for (int i = 0; i < total; ++i) {
        const float gamma = powf(0.5f, (average - pF[i]) * inv_average);
        const float v_normalized = pV[i] * 0.00392156862745098f;    // pV[i] / 255.0f;
        const float v_powered = powf(v_normalized, gamma);
        pV[i] = v_powered * 255.0f;
    }

    V_float.convertTo(channels[2], CV_8U);

    cv::Mat result;
    cv::merge(channels, result);
    cv::cvtColor(result, result, cv::COLOR_HSV2BGR);
    return result;
}


void FA::GetGrayIntegralImage(unsigned char* Src, int* Integral, int Width, int Height, int Stride)
{
	std::memset(Integral, 0, (Width + 1) * sizeof(int));		   //	第一行都为0
	for (int Y = 0; Y < Height; Y++) {
		unsigned char* LinePS = Src + Y * Stride;
		int* LinePL = Integral + Y * (Width + 1) + 1;				//	上一行位置			
		int* LinePD = Integral + (Y + 1) * (Width + 1) + 1;			//	当前位置，注意每行的第一列的值都为0
		LinePD[-1] = 0;												//	第一列的值为0
		for (int X = 0, Sum = 0; X < Width; X++)
		{
			Sum += LinePS[X];										//	行方向累加
			LinePD[X] = LinePL[X] + Sum;							//	更新积分图
		}
	}
}


void FA::GetRGBIntegralImage(unsigned char* Src, int* Integral, int Width, int Height, int Stride)
{
	const int Channel = 3;
	size_t tmp = (Width + 1) * Channel;
	std::memset(Integral, 0, tmp * sizeof(int));
	for (int Y = 0; Y < Height; Y++)
	{
		unsigned char* LinePS = Src + Y * Stride;
		int* LinePL = Integral + Y * tmp + Channel;
		int* LinePD = Integral + (Y + 1) * tmp + Channel;
		LinePD[-3] = 0; LinePD[-2] = 0; LinePD[-1] = 0;
		for (int X = 0, SumB = 0, SumG = 0, SumR = 0; X < Width; X++)
		{
			SumB += LinePS[0];
			SumG += LinePS[1];
			SumR += LinePS[2];
			LinePD[0] = LinePL[0] + SumB;
			LinePD[1] = LinePL[1] + SumG;
			LinePD[2] = LinePL[2] + SumR;
			LinePS += Channel;
			LinePL += Channel;
			LinePD += Channel;
		}
	}
}



void FA::BoxBlur(unsigned char* Src, unsigned char* Dest, int Width, int Height, int Stride, int Radius)
{
	int Channel = Stride / Width;
	int IntegralSize = (Width + 1) * (Height + 1) * Channel;
	int* Integral = new int[IntegralSize];
	if (Channel == 1)
	{
		FA::GetGrayIntegralImage(Src, Integral, Width, Height, Stride);
#pragma omp parallel for
		for (int Y = 0; Y < Height; Y++)
		{
			int Y1 = std::max(Y - Radius, 0);
			int Y2 = std::min(Y + Radius + 1, Height);
			int* LineP1 = Integral + Y1 * (Width + 1);
			int* LineP2 = Integral + Y2 * (Width + 1);
			unsigned char* LinePD = Dest + Y * Stride;
			for (int X = 0; X < Width; X++)
			{
				int X1 = std::max(X - Radius, 0);
				int X2 = std::min(X + Radius + 1, Width);
				int Sum = LineP2[X2] - LineP1[X2] - LineP2[X1] + LineP1[X1];
				int PixelCount = (X2 - X1) * (Y2 - Y1);					//	有效的像素数
				LinePD[X] = (Sum + (PixelCount >> 1)) / PixelCount;		//	四舍五入
			}
		}
	}
	else if (Channel == 3)
	{
		FA::GetRGBIntegralImage(Src, Integral, Width, Height, Stride);
#pragma omp parallel for
		for (int Y = 0; Y < Height; Y++)
		{
			int Y1 = std::max(Y - Radius, 0);
			int Y2 = std::min(Y + Radius + 1, Height);
			int* LineP1 = Integral + Y1 * (Width + 1) * 3;
			int* LineP2 = Integral + Y2 * (Width + 1) * 3;
			unsigned char* LinePD = Dest + Y * Stride;
			for (int X = 0; X < Width; X++)
			{
				int X1 = std::max(X - Radius, 0);
				int X2 = std::min(X + Radius + 1, Width);
				int Index1 = X1 * 3;
				int Index2 = X2 * 3;
				int SumB = LineP2[Index2 + 0] - LineP1[Index2 + 0] - LineP2[Index1 + 0] + LineP1[Index1 + 0];
				int SumG = LineP2[Index2 + 1] - LineP1[Index2 + 1] - LineP2[Index1 + 1] + LineP1[Index1 + 1];
				int SumR = LineP2[Index2 + 2] - LineP1[Index2 + 2] - LineP2[Index1 + 2] + LineP1[Index1 + 2];
				int PixelCount = (X2 - X1) * (Y2 - Y1);
				LinePD[0] = (SumB + (PixelCount >> 1)) / PixelCount;
				LinePD[1] = (SumG + (PixelCount >> 1)) / PixelCount;
				LinePD[2] = (SumR + (PixelCount >> 1)) / PixelCount;
				LinePD += 3;
			}
		}
	}
	delete[] Integral;
}



int FA::getMediaValue(const int hist[], int thresh) {
	int sum = 0;
	for (int i = 0; i < 256; i++) {
		sum += hist[i];
		if (sum >= thresh) {
			return i;
		}
	}
	return 255;
}


cv::Mat FA::fastMedianBlur(const cv::Mat& src, int diameter)
{
	int row = src.rows;
	int col = src.cols;
	cv::Mat dst(row, col, CV_8UC1);
	int Hist[256] = { 0 };
	int radius = (diameter - 1) / 2;
	int windowSize = diameter * diameter;
	int threshold = windowSize / 2 + 1;
	uchar* srcData = src.data;
	uchar* dstData = dst.data;
	int right = col - radius;
	int bot = row - radius;
	for (int j = radius; j < bot; j++) {
		for (int i = radius; i < right; i++) {
			//每一行第一个待滤波元素建立直方图
			if (i == radius) {
				std::memset(Hist, 0, sizeof(Hist));
				for (int y = j - radius; y <= std::min(j + radius, row); y++) {
					for (int x = i - radius; x <= std::min(i + radius, col); x++) {
						uchar val = srcData[y * col + x];
						Hist[val]++;
					}
				}
			}
			else {
				int L = i - radius - 1;
				int R = i + radius;
				for (int y = j - radius; y <= std::min(j + radius, row); y++) {
					//更新左边一列
					Hist[srcData[y * col + L]]--;
					//更新右边一列
					Hist[srcData[y * col + R]]++;
				}
			}
			uchar medianVal = FA::getMediaValue(Hist, threshold);
			dstData[j * col + i] = medianVal;
		}
	}
	//边界直接赋值
	for (int i = 0; i < col; i++) {
		for (int j = 0; j < radius; j++) {
			int id1 = j * col + i;
			int id2 = (row - j - 1) * col + i;
			dstData[id1] = srcData[id1];
			dstData[id2] = srcData[id2];
		}
	}
	for (int i = radius; i < row - radius - 1; i++) {
		for (int j = 0; j < radius; j++) {
			int id1 = i * col + j;
			int id2 = i * col + col - j - 1;
			dstData[id1] = srcData[id1];
			dstData[id2] = srcData[id2];
		}
	}
	return dst;
}



uchar FA::adaptiveProcess(const cv::Mat& im, int row, int col, int kernelSize, int maxSize)
{
	while (true) {
		int halfSize = kernelSize / 2;
		int size = kernelSize * kernelSize;
		std::vector<uchar> pixels(size);
		int index = 0;
		for (int a = -halfSize; a <= halfSize; a++) {
			for (int b = -halfSize; b <= halfSize; b++) {
				pixels[index++] = im.at<uchar>(row + a, col + b);
			}
		}
		std::sort(pixels.begin(), pixels.end());
		auto min = pixels[0];
		auto max = pixels[size - 1];
		auto med = pixels[size / 2];
		auto zxy = im.at<uchar>(row, col);
		if (med > min && med < max) {
			if (zxy > min && zxy < max) {
				return zxy;
			}
			else {
				return med;
			}
		}
		else {
			kernelSize += 2;
			if (kernelSize > maxSize) {
				return med;
			}
		}
	}
}


cv::Mat FA::Adaptive_Median_Filter(const cv::Mat& src)
{
	cv::Mat dst;
	int minSize = 3; // 滤波器窗口的起始大小
	int maxSize = 7; // 滤波器窗口的最大尺寸
	copyMakeBorder(src, dst, maxSize / 2, maxSize / 2, maxSize / 2, maxSize / 2, cv::BORDER_REFLECT);
	int rows = dst.rows;
	int cols = dst.cols;

	for (int j = maxSize / 2; j < rows - maxSize / 2; j++) {
		uchar* rowPtr = dst.ptr<uchar>(j);
		for (int i = maxSize / 2; i < cols - maxSize / 2; i++) {
			rowPtr[i] = adaptiveProcess(dst, j, i, minSize, maxSize);
		}
	}

	// 裁剪边界
	cv::Rect roi(maxSize / 2, maxSize / 2, src.cols, src.rows);
	return dst(roi);
}



cv::Mat FA::getimage(cv::Mat& a)
{
	int hei = a.rows;
	int wid = a.cols;
	cv::Mat I(hei, wid, CV_64FC1);
	a.convertTo(I, CV_64FC1, 1.0 / 255.0);
	return I;
}


cv::Mat FA::cumsum(cv::Mat& imSrc, int rc)
{
	if (!imSrc.data)
	{
		std::cout << "no data input!\n";
	}
	int hei = imSrc.rows;
	int wid = imSrc.cols;
	cv::Mat imCum = imSrc.clone();
	if (rc == 1)
	{
		for (int i = 1; i < hei; i++)
		{
			double* rowPtr = imCum.ptr<double>(i);
			double* prevRowPtr = imCum.ptr<double>(i - 1);
			for (int j = 0; j < wid; j++)
			{
				rowPtr[j] += prevRowPtr[j];
			}
		}
	}
	else if (rc == 2)
	{
		for (int i = 0; i < hei; i++)
		{
			double* rowPtr = imCum.ptr<double>(i);
			for (int j = 1; j < wid; j++)
			{
				rowPtr[j] += rowPtr[j - 1];
			}
		}
	}
	return imCum;
}


cv::Mat FA::boxfilter(cv::Mat& imSrc, int r)
{
	int hei = imSrc.rows;
	int wid = imSrc.cols;
	cv::Mat imDst = cv::Mat::zeros(hei, wid, CV_64FC1);
	cv::Mat imCum = cumsum(imSrc, 1);

	for (int i = 0; i < r + 1; i++)
	{
		double* dstRowPtr = imDst.ptr<double>(i);
		double* cumRowPtr = imCum.ptr<double>(i + r);
		for (int j = 0; j < wid; j++)
		{
			dstRowPtr[j] = cumRowPtr[j];
		}
	}

	for (int i = r + 1; i < hei - r; i++)
	{
		double* dstRowPtr = imDst.ptr<double>(i);
		double* cumRowPtr1 = imCum.ptr<double>(i + r);
		double* cumRowPtr2 = imCum.ptr<double>(i - r - 1);
		for (int j = 0; j < wid; j++)
		{
			dstRowPtr[j] = cumRowPtr1[j] - cumRowPtr2[j];
		}
	}

	for (int i = hei - r; i < hei; i++)
	{
		double* dstRowPtr = imDst.ptr<double>(i);
		double* cumRowPtr = imCum.ptr<double>(hei - 1);
		double* cumRowPtr2 = imCum.ptr<double>(i - r - 1);
		for (int j = 0; j < wid; j++)
		{
			dstRowPtr[j] = cumRowPtr[j] - cumRowPtr2[j];
		}
	}

	imCum = cumsum(imDst, 2);

	for (int i = 0; i < hei; i++)
	{
		double* dstRowPtr = imDst.ptr<double>(i);
		double* cumRowPtr = imCum.ptr<double>(i);
		for (int j = 0; j < r + 1; j++)
		{
			dstRowPtr[j] = cumRowPtr[j + r];
		}
	}

	for (int i = 0; i < hei; i++)
	{
		double* dstRowPtr = imDst.ptr<double>(i);
		double* cumRowPtr1 = imCum.ptr<double>(i);
		double* cumRowPtr2 = imCum.ptr<double>(i);
		for (int j = r + 1; j < wid - r; j++)
		{
			dstRowPtr[j] = cumRowPtr1[j + r] - cumRowPtr2[j - r - 1];
		}
	}

	for (int i = 0; i < hei; i++)
	{
		double* dstRowPtr = imDst.ptr<double>(i);
		double* cumRowPtr = imCum.ptr<double>(i);
		double* cumRowPtr2 = imCum.ptr<double>(i);
		for (int j = wid - r; j < wid; j++)
		{
			dstRowPtr[j] = cumRowPtr[wid - 1] - cumRowPtr2[j - r - 1];
		}
	}

	return imDst;
}


cv::Mat FA::guidedfilter_kernel(cv::Mat& I, cv::Mat& p, int r, double eps)
{
	int hei = I.rows;
	int wid = I.cols;
	cv::Mat one = cv::Mat::ones(hei, wid, CV_64FC1);
	cv::Mat N = boxfilter(one, r);

	cv::Mat mean_I = boxfilter(I, r) / N;
	cv::Mat mean_p = boxfilter(p, r) / N;
	cv::Mat mul_Ip = I.mul(p);
	cv::Mat mean_Ip = boxfilter(mul_Ip, r) / N;
	cv::Mat cov_Ip = mean_Ip - mean_I.mul(mean_p);

	cv::Mat mul_II = I.mul(I);
	cv::Mat mean_II = boxfilter(mul_II, r) / N;
	cv::Mat var_I = mean_II - mean_I.mul(mean_I);

	cv::Mat a = cov_Ip / (var_I + eps);
	cv::Mat b = mean_p - a.mul(mean_I);

	cv::Mat mean_a = boxfilter(a, r) / N;
	cv::Mat mean_b = boxfilter(b, r) / N;

	cv::Mat q = mean_a.mul(I) + mean_b;

	return q;
}



cv::Mat FA::guidedFilter_kernel2(cv::Mat I, cv::Mat p, int r, double eps)
{
	I.convertTo(I, CV_64FC1);
	p.convertTo(p, CV_64FC1);

	int hei = I.rows;
	int wid = I.cols;

	cv::Mat N;
	boxFilter(cv::Mat::ones(hei, wid, I.type()), N, CV_64FC1, cv::Size(r, r));

	cv::Mat mean_I;
	boxFilter(I, mean_I, CV_64FC1, cv::Size(r, r));

	cv::Mat mean_p;
	boxFilter(p, mean_p, CV_64FC1, cv::Size(r, r));

	cv::Mat mean_Ip;
	boxFilter(I.mul(p), mean_Ip, CV_64FC1, cv::Size(r, r));

	cv::Mat cov_Ip = mean_Ip - mean_I.mul(mean_p);

	cv::Mat mean_II;
	boxFilter(I.mul(I), mean_II, CV_64FC1, cv::Size(r, r));

	cv::Mat var_I = mean_II - mean_I.mul(mean_I);

	cv::Mat a = cov_Ip / (var_I + eps);

	cv::Mat b = mean_p - a.mul(mean_I);

	cv::Mat mean_a;
	boxFilter(a, mean_a, CV_64FC1, cv::Size(r, r));
	mean_a = mean_a / N;

	cv::Mat mean_b;
	boxFilter(b, mean_b, CV_64FC1, cv::Size(r, r));
	mean_b = mean_b / N;

	cv::Mat q = mean_a.mul(I) + mean_b;

	return q;
}



cv::Mat FA::guidedFilter_kernel3(cv::Mat I, cv::Mat p, int r, double eps)
{
	// 计算I的均值平滑
	cv::Size winSize(r, r);

	cv::Mat mean_I;
	cv::blur(I, mean_I, winSize);

	// 计算p的均值平滑
	cv::Mat mean_p;
	cv::blur(p, mean_p, winSize);

	// 计算I*I的均值平滑
	cv::Mat I_squared = I.mul(I);
	cv::Mat mean_II;
	cv::blur(I_squared, mean_II, winSize);

	// 计算I*p的均值平滑
	cv::Mat I_p = I.mul(p);
	cv::Mat mean_Ip;
	cv::blur(I_p, mean_Ip, winSize);

	// 计算方差
	cv::Mat var_I = mean_II - mean_I.mul(mean_I);

	// 计算协方差
	cv::Mat cov_Ip = mean_Ip - mean_I.mul(mean_p);

	// 计算相关因子a
	cv::Mat a = cov_Ip / (var_I + eps);

	// 计算相关因子b
	cv::Mat b = mean_p - a.mul(mean_I);

	// 对a进行均值平滑
	cv::Mat mean_a;
	cv::blur(a, mean_a, winSize);

	// 对b进行均值平滑
	cv::Mat mean_b;
	cv::blur(b, mean_b, winSize);

	// 计算导向滤波结果
	cv::Mat q = mean_a.mul(I) + mean_b;

	return q;
}



cv::Mat FA::guidedFilter(cv::Mat& src, int r, double eps)
{
	if (src.empty() || src.channels() != 3) {
		std::cerr << "Could not open or find the image!" << std::endl;
		exit(-1);
	}
	cv::Mat imageIP;
	src.convertTo(imageIP, CV_32FC3, 1 / 255);
	cv::Mat guideFilter_img = FA::guidedFilter_kernel3(imageIP, imageIP, r, eps);

	for (int i = 0; i < guideFilter_img.rows; ++i) {
		for (int j = 0; j < guideFilter_img.cols; ++j) {
			cv::Vec3b& pixel = guideFilter_img.at<cv::Vec3b>(i, j);
			for (int c = 0; c < 3; ++c) {
				float pixelValue = pixel[c] * 255;
				if (pixelValue > 255) {
					pixelValue = 255;
				}
				pixel[c] = static_cast<uchar>(std::round(pixelValue));
			}
		}
	}
	guideFilter_img.convertTo(guideFilter_img, CV_8U);
	return guideFilter_img;
}
